# 遥控加传图
"""
w: 前进
s: 后退
a: 左转
d: 右转
q: 退出系统
e: 停止运动
r: 开启一个传图线程并等待客户端的连接
"""
import serial
import time
import sys
import tty
import termios
import socket
import threading
import cv2
import errno
from socket import error as SocketError


# 用于捕获用户键盘输入的两个方法
def readchar():
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    try:
        tty.setraw(sys.stdin.fileno())
        ch = sys.stdin.read(1)
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
    return ch

def readkey(getchar_fn=None):
    getchar = getchar_fn or readchar
    c1 = getchar()
    if ord(c1) != 0x1b:
        return c1
    c2 = getchar()
    if ord(c2) != 0x5b:
        return c1
    c3 = getchar()
    return chr(0x10 + ord(c3) - 65)


ser = serial.Serial('/dev/ttyS0', 115200)  # 声明串口
if not ser.isOpen:  # 确认串口已打开
    ser.open()
ser.write(b'serial test')  # 发送测试数据,可用于激活移动平台的串口控制功能

def back():
    ser.write(bytes.fromhex('FF FE 24 24 01 01 00 00 00 00'))
    time.sleep(0.3)


def go():
    ser.write(bytes.fromhex('FF FE 24 24 00 00 00 00 00 00'))
    time.sleep(0.3)


def right():
    ser.write(bytes.fromhex('FF FE 24 24 01 00 00 00 00 00'))
    time.sleep(0.3)


def left():
    ser.write(bytes.fromhex('FF FE 24 24 00 01 00 00 00 00'))
    time.sleep(0.3)


def stop():
    ser.write(bytes.fromhex('FF FE 00 00 00 00 00 00 00 00'))
    time.sleep(0.1)

def PicSend(s):
    """从摄像头获取数据并发送"""
    print('视频输出线程已就绪')
    conn, addr = s.accept()  # 等待建立客户端连接
    print('连接地址:', addr)  # 打印客户端IP
    cap = cv2.VideoCapture(0)  # 定义一个摄像头用于拍照
    sendCloseFlag = 1  # 关闭发送标志位
    try:  # 屏蔽掉客户端断开时的报错
        while sendCloseFlag:
            ret, frame = cap.read()  # 拍一张图片
            data = cv2.imencode(".jpg", frame)[1].tobytes()  # 将图片转换成二进制流（为了传输）
            conn.send(str(len(data)).encode('utf-8'))  # 发送图片的长度
            waitACK = conn.recv(1024).decode()  # 等待接收方的应答
            while waitACK != 'ready':
                print('等待应答')
                waitACK = conn.recv(1024).decode()
                if waitACK == 'stop':  # 如果从机发送停止命令，主机关闭socket
                    sendCloseFlag = 0
                    break
            conn.send(data)  # 应答后发送图片数据
            print('over send')
        conn.close()
    except SocketError as e:
        if e.errno != errno.ECONNRESET:
            raise
        pass


link = ("192.168.2.192", 12345)
skt = socket.socket()  # 创建 socket 对象
skt.bind(link)  # 绑定端口
skt.listen(5)  # 设置监听数量
while True:
    key = readkey()
    if key == 'w':
        print('Forward')
        go()
    elif key == 's':
        print('Backward')
        back()
    elif key == 'a':
        print('Left')
        left()
    elif key == 'd':
        print('Right')
        right()
    elif key == 'e':
        print('Stop')
        stop()
    elif key == 'r':
        print('准备开启传图线程')
        threading.Thread(target=PicSend, args=(skt,)).start()
    elif key == 'q':
        print('Quit!')
        break

ser.close()

